/*
 * regelaar.c
 *
 *  Created on: 21 jun. 2011
 *      Author: dirkjan
 */

#include "bluetooth.h"		// alleen voor testen! todo uiteindelijk weghalen!
#include "regelaar.h"
#include "TWI_Master.h"
#include "motorsturing.h"

static signed char 	accel_xdata,
					accel_ydata,
					accel_zdata,
					ovf_counter,
					CommandStaart,
					CommandDraai,
					lock_motors,
					outputStaart,
					outputLift,
					outputDraai;

static unsigned char CommandLift;

static unsigned char 	ugxl, ugxh,
						ugyl, ugyh,
						ugzl, ugzh,
						new_gyro_data;

static signed int gyrox, gyroy, gyroz;
static unsigned int call_count, sensor_access_count, calls, sensor_accesses;
static unsigned int pGain = 2, iGain, dGain;



void sensortest(){

	signed char x, y, z;

	x	=	get_accelero_data('x');
	y	= 	get_accelero_data('y');
	z	=	get_accelero_data('z');

	bluetooth_send_text("\r\nx-data: ");
	bluetooth_send_nr((int)x);
	bluetooth_send_text("\r\ny-data: ");
	bluetooth_send_nr((int)y);
	bluetooth_send_text("\r\nz-data: ");
	bluetooth_send_nr((int)z);

	bluetooth_send_text("\r\ngiro x: ");
	/*bluetooth_send_nr(ugxh);
	bluetooth_send_char('-');
	bluetooth_send_nr(ugxl);
	bluetooth_send_char(' ');
	*/bluetooth_send_nr(gyrox >> 8);
	bluetooth_send_text("\r\ngyro y: ");
	/*bluetooth_send_nr(ugyh );
	bluetooth_send_char('-');
	bluetooth_send_nr(ugyl);
	bluetooth_send_char(' ');
	*/bluetooth_send_nr(gyroy >> 8);
	bluetooth_send_text("\r\ngiro z: ");
	/*bluetooth_send_nr(ugzh);
	bluetooth_send_char('-');
	bluetooth_send_nr(ugzl);
	bluetooth_send_char(' ');
	*/bluetooth_send_nr(gyroz >> 8);
	/*
	bluetooth_send_text("\r\ng-z:  ");
	bluetooth_send_nr((int)ugzh);
	bluetooth_send_char(' ');
	bluetooth_send_nr((int)ugzl);
	bluetooth_send_char(' ');
	bluetooth_send_nr(gyroz);

	bluetooth_send_text("\r\nstate info: ");
	bluetooth_send_nr(TWI_Get_State_Info());

	bluetooth_send_text("\r\ncall: ");
	bluetooth_send_nr(calls);
	bluetooth_send_text("\r\nfunc: ");
	bluetooth_send_nr(sensor_accesses);
	bluetooth_send_text("\r\n");
	*/

}

void regelaar_init(){
	lock_motors = 0;
	init_motors();
	TWI_Master_Initialise();
	while (write_to_reg(ACCEL_WRITE_ADDRESS, ACCEL_CTRL_1, ACCEL_NO_SLEEP_MASK));		// zorgen dat de accelero niet in slaap valt
	while (write_to_reg(GYRO_WRITE_ADDRESS, GYRO_CTRL_1, GYRO_INIT_MASK));				// zorgen dat de gyro niet in slaap valt
}

void lock_motor_driver(signed char lock){
	lock_motors = lock;
}
unsigned char write_to_reg(unsigned char dev_id, unsigned char reg_addr, unsigned char data){
	if( ! TWI_Transceiver_Busy()){
		unsigned char message_buf[3];
		message_buf[0]	=	dev_id;
		message_buf[1]	=	reg_addr;
		message_buf[2]	=	data;
		TWI_Start_Transceiver_With_Data( message_buf, 3 );
		return 0;
	}
	return 1;
}

unsigned char get_single_data_from_sensor(unsigned char reg_addr, unsigned char *data_ptr, unsigned char dev_id){
	if( ! TWI_Transceiver_Busy()){
		unsigned char message_buf[4];
		message_buf[0]	=	dev_id;
		message_buf[1]	=	reg_addr;
		message_buf[2]	=	dev_id | I2C_READ_MASK;
		TWI_Start_Transceiver_With_Data(message_buf, 4);
		return 0;
	}
	return 1;
}

unsigned char get_multi_data_from_sensor(){
	return 1;
}

unsigned char get_accel_data_from_sensor(unsigned char reg_addr, signed char *data_ptr, unsigned char dev_id){
	static unsigned char accel_state=0;
	unsigned char dev_write_address, dev_read_address;
	unsigned char messageBuf[3];
	dev_write_address	= dev_id;
	dev_read_address	= dev_id | 1;

	if( ! TWI_Transceiver_Busy()){
		switch (accel_state){
		case 0:
			messageBuf[0] = dev_write_address; // The first byte must always consit of General Call code or the TWI slave address.
			messageBuf[1] = reg_addr;             		// The first byte is used for commands.
			TWI_Start_Transceiver_With_Data( messageBuf, 2 );
			accel_state++;
			break;
		case 1:
			messageBuf[0] = dev_read_address; // The first byte must always consit of General Call code or the TWI slave address.
			TWI_Start_Transceiver_With_Data( messageBuf, 2 );
			accel_state++;
			break;
		case 2:
			TWI_Get_Data_From_Transceiver( messageBuf, 2 );
			/*if( (data_ptr == &accel_xdata) || (data_ptr == &accel_ydata) || (data_ptr == &accel_zdata) ){
				(* (signed char* )data_ptr)	= (signed char) messageBuf[1];
			}*/

			(*data_ptr) = (signed char)messageBuf[1];

			accel_state=4;
			ovf_counter	= 0;
			break;
		}
	}

	if (accel_state == 4){
		accel_state = 0;
		return 1;
	}else
		return 0;
}


unsigned char get_gyro_data_from_sensor(unsigned char reg_addr, unsigned char *data_ptr, unsigned char dev_id){
	static unsigned char state=0;
	unsigned char dev_write_address, dev_read_address;
	unsigned char messageBuf[3];
	dev_write_address	= dev_id;
	dev_read_address	= dev_id | 1;

	if( ! TWI_Transceiver_Busy()){
		switch (state){
		case 0:
			messageBuf[0] = dev_write_address; // The first byte must always consit of General Call code or the TWI slave address.
			messageBuf[1] = reg_addr;             		// The first byte is used for commands.
			TWI_Start_Transceiver_With_Data( messageBuf, 2 );
			state++;
			break;
		case 1:
			messageBuf[0] = dev_read_address; // The first byte must always consit of General Call code or the TWI slave address.
			TWI_Start_Transceiver_With_Data( messageBuf, 2 );
			state++;
			break;
		case 2:
			TWI_Get_Data_From_Transceiver( messageBuf, 2 );
			/*if( (data_ptr == &accel_xdata) || (data_ptr == &accel_ydata) || (data_ptr == &accel_zdata) ){
				(* (signed char* )data_ptr)	= (signed char) messageBuf[1];
			}*/

			(*data_ptr) = messageBuf[1];

			state=4;
			ovf_counter	= 0;
			break;
		}
	}

	if (state == 4){
		state = 0;
		return 1;
	}else
		return 0;
}

signed char get_accelero_data(char axis){
	signed char data = 42;

	switch(axis){
		case 'x':
			data	= accel_xdata;
			break;
		case 'y':
			data	= accel_ydata;
			break;
		case 'z':
			data	= accel_zdata;
			break;
	}
	return data;
}

unsigned char has_accel_data_changed(){
	char	changed	= 1;
	return changed;
}

void read_sensors(){
	static unsigned char i2c_read_state=9;
	unsigned char acceldata_request=ACCEL_XDATA, dev_id=ACCEL_WRITE_ADDRESS;
	unsigned char *gyrodata_ptr=0;
	signed char *acceldata_ptr=0;

	switch (i2c_read_state){
		case 0:
			gyroz	= (signed int) ((ugzh << 8) | ugzl);	// ugzh lezen is gelukt, set gyroz
			dev_id=ACCEL_WRITE_ADDRESS;
			acceldata_request=ACCEL_XDATA;
			acceldata_ptr=&accel_xdata;
			break;
		case 1:
			//accel_xdata	= (signed char) uax;
			dev_id=ACCEL_WRITE_ADDRESS;
			acceldata_request=ACCEL_YDATA;
			acceldata_ptr=&accel_ydata;
			break;
		case 2:
			//accel_ydata	= (signed char) uay;
			dev_id=ACCEL_WRITE_ADDRESS;
			acceldata_request=ACCEL_ZDATA;
			acceldata_ptr=&accel_zdata;
			break;
		case 3:
			//accel_zdata	= (signed char) uaz;
			dev_id=GYRO_WRITE_ADDRESS;
			acceldata_request=GYRO_XDATA_L;
			gyrodata_ptr	=&ugxl;
			break;
		case 4:
			dev_id=GYRO_WRITE_ADDRESS;
			acceldata_request=GYRO_XDATA_H;
			gyrodata_ptr	=&ugxh;
			break;
		case 5:
			gyrox	= (signed int) ((ugxh << 8) | ugxl);	//ugxh uitlezen is gelukt, set gyrox
			dev_id=GYRO_WRITE_ADDRESS;
			acceldata_request=GYRO_YDATA_L;
			gyrodata_ptr	=&ugyl;
			break;
		case 6:
			dev_id=GYRO_WRITE_ADDRESS;
			acceldata_request=GYRO_YDATA_H;
			gyrodata_ptr	=&ugyh;
			break;
		case 7:
			gyroy	= (signed int) ((ugyh << 8) | ugyl);	//ugyh uitlezen is gelukt, set gyroy
			dev_id=GYRO_WRITE_ADDRESS;
			acceldata_request=GYRO_ZDATA_L;
			gyrodata_ptr	=&ugzl;
			break;
		case 8:
			dev_id=GYRO_WRITE_ADDRESS;
			acceldata_request=GYRO_ZDATA_H;
			gyrodata_ptr	=&ugzh;
			break;
		case 9:
			dev_id=GYRO_WRITE_ADDRESS;
			acceldata_request=GYRO_DATA_AVAILABLE;
			gyrodata_ptr	=&new_gyro_data;	// TODO Status Register wordt nog niet gemaskt en is dus waarschijnlijk nooit nul! Mask met GYRO_DATA_AVAILABLE
			break;
	}


	if ( (i2c_read_state > 2 ) && get_gyro_data_from_sensor(acceldata_request, gyrodata_ptr, dev_id)){
		i2c_read_state++;
		if (i2c_read_state>9)
			i2c_read_state = 0;
	}
	if( (i2c_read_state < 3) && get_accel_data_from_sensor(acceldata_request, acceldata_ptr, dev_id)){
		i2c_read_state++;
	}
	if(!new_gyro_data && (i2c_read_state >2) && (i2c_read_state < 9)){
		i2c_read_state=9;
	}

}
void regelaar_task(unsigned char ovf){
	if(ovf){
		ovf_counter++;
	}

	call_count++;
	if(new_gyro_data)
		sensor_access_count++;
	if(call_count > 1000){
		call_count=0;
		calls++;
	}
	if(sensor_access_count > 1000){
		sensor_access_count=0;
		sensor_accesses++;
	}


	read_sensors();

	if (lock_motors||outputLift==0){
		set_speed_motor(STAARTROTOR,0);
		set_speed_motor(ONDERROTOR,0);
		set_speed_motor(BOVENROTOR,0);
	}else{
		set_tail_rotor(outputStaart, outputLift);
		set_main_rotors(outputLift, outputDraai);


		/*
		set_speed_motor(STAARTROTOR,(unsigned char)outputStaart);
		if (outputLift-outputDraai>0)
			set_speed_motor(ONDERROTOR,(unsigned char)(outputLift-outputDraai));
		else
			set_speed_motor(BOVENROTOR,outputLift);
		if (outputLift+outputDraai>0)
			set_speed_motor(BOVENROTOR,(unsigned char)(outputLift+outputDraai));
		else
			set_speed_motor(BOVENROTOR,outputLift);
			*/
	}
	/*
	if(ovf_counter >= 100){
		if( ! TWI_Transceiver_Busy()){
			write_to_reg(GYRO_WRITE_ADDRESS, GYRO_CTRL_4, GYRO_SCALE_MAX);
			bluetooth_send_text("scale change\r\n");
		}
		ovf_counter	= 0;
	}
	*/
	PID_update();
}

void PID_update(){
	static signed int	errorStaart, errorLift, errorDraai;
	signed int pTerm, iTerm, dTerm;

	errorDraai		= CommandDraai - (signed char)(gyroz >> 8);
	pTerm			= errorDraai * pGain;
	if(pTerm > 126)
		pTerm	= 126;
	else if (pTerm < -127)
		pTerm	= -127;

	outputStaart	= CommandStaart;
	outputLift		= 155+CommandLift;		//gaat van 155 tot 255

	outputDraai		= (signed char) pTerm;
}

signed char get_gyro_data(char as){
	signed char data = 42;

	switch(as){
		case 'x':
			data	= (signed char)(gyrox >> 8);
			break;
		case 'y':
			data	= (signed char)(gyroy >> 8);
			break;
		case 'z':
			data	= (signed char)(gyroz >> 8);
			break;
	}
	return data;
}

signed char get_temp(){
	return 3;
}


void set_offset_draai(signed char offset){
	if (offset>DRAAI_OFFSET_MAX)
		CommandDraai=DRAAI_OFFSET_MAX;
	else if (offset<-DRAAI_OFFSET_MAX)
		CommandDraai=-DRAAI_OFFSET_MAX;
	else
		CommandDraai = offset;
	//CommandDraai/=2;
	//CommandDraai+=25;
}

void set_offset_lift(signed char offset){
	if (offset>100)
		CommandLift = 100;
	else
		CommandLift = offset;

}

void set_offset_staart(signed char offset){
	if (offset>100)
		CommandStaart = 100;
	else
		CommandStaart = offset;
}


void set_P(unsigned int value){
	pGain	= value;
}

void set_I(unsigned int value){
	iGain	= value;
}

void set_D(unsigned int value){
	dGain	= value;
}
